import math

def stateDistanceWithoutControls(s1, s2):
    da = s1.arm1.a-s2.arm1.a
    daDot = s1.arm1.aDot-s2.arm1.aDot
    d = math.sqrt(da*da + daDot*daDot)
    return d

def stateDistance(s1, s2):
    dc1 = s1.arm1.c1-s2.arm1.c1
    dc2 = s1.arm1.c2-s2.arm1.c2
    da = s1.arm1.a-s2.arm1.a
    daDot = s1.arm1.aDot-s2.arm1.aDot
    d = math.sqrt(dc1*dc1 + dc2*dc2 + da*da + daDot*daDot)
    return d

class Arm1:
    def __init__(self):
        self.c1 = 0
        self.c2 = 0
        self.a = 0
        self.aDot = 0
        
    def stiffness(self):
        return 0.5*(self.c1+self.c2) - math.fabs(self.c1-self.c2)

class State:
    def __init__(self):
        self.arm1 = Arm1()
        self.modelExists = False
        
    def interpolate(self, other, f):
        s = State()
        s.arm1.c1 = f*self.arm1.c1 + (1.0-f)*other.arm1.c1 
        s.arm1.c2 = f*self.arm1.c2 + (1.0-f)*other.arm1.c2 
        s.arm1.a = f*self.arm1.a + (1.0-f)*other.arm1.a
        s.arm1.aDot = f*self.arm1.aDot + (1.0-f)*other.arm1.aDot
        return s
        
    def __str__(self):
        return "c1: {0: 7.3f}, c2: {1: 7.3f}, angle: {2: 7.3f}, dot angle {3: 7.3f}".format(self.arm1.c1, self.arm1.c2, self.arm1.a, self.arm1.aDot)
